#include <servo.h>

#define SWITCH_ON   0x07
#define SHUTDOWN    0x06
#define FAULT_RESET 0x80
#define QUICK_STOP  0x02
#define NONE        0x00

#define DISABLE_OPERATION 0x07
#define ENABLE_OPERATION  0x0f

#define TARGETMODE      0x6060
#define TARGETCONTROL   0x6040     // write
#define TARGETPOS       0x607A
#define TARGETVEL       0x60FF
#define TARGETMAXVEL    0x6080
#define TARGETTVEL      0x6081
#define TARGETMAXTOR    0x6072
#define	TARGETTOR       0x6071

#define ACTUALMODE      0x6061
#define ERRORCODE       0x603F     // read
#define ACTUALSTATUS    0x6041
#define ACTUALPOS       0x6064
#define ACTUALVEL       0x606C

Servo::Servo()
{
    status_ = 0x00;
}

Servo::~Servo() {}

bool Servo::Parse(EcXml &ecxml, ec_master_t* master, int index){
    (void)master;
    (void)index;
    (void)ecxml;

    if(!ecxml.GetServoSyncInfo(&m_SyncInfo_))
    {
        m_SyncInfo_ = nullptr;
        return false;
    }
    return true;
}

bool Servo::StatusTransite()
{
    switch (txpdo_.status & 0x6F)
    {
        case 0x00:  // Not Ready To Switch ON
        case 0x20:
            break;

        case 0x40:  // Switch On Disable
        case 0x60:  // Switch On Disable
            rxpdo_.control = SHUTDOWN;
            break;

        case 0x21:  // Ready to Switch On
            rxpdo_.control = SWITCH_ON;
            break;

        case 0x23:  // Switch On
            if ( delay_ >= 500 )
            {
                rxpdo_.pos = txpdo_.pos;
                rxpdo_.vel = txpdo_.vel;

                status_ = 0x02;
                if ( enable_ )
                {
                    rxpdo_.control = ENABLE_OPERATION;
                }
            }
            
            if (delay_ < 500) ++ delay_;
            if (reset_) reset_ = false;
            break;

        case 0x25:
            break;

        case 0x27:  // Operation Enable
            if ( !enable_ )
            {
                rxpdo_.control = DISABLE_OPERATION;
            }
            status_ = 0x04;
            break;

        default:    // reset
            // if (status_ != 0x00){
            status_ = 0x08;
            // }

            if ( reset_ )
            {
                delay_ = 0;
                rxpdo_.control = FAULT_RESET;
            }
            else
            {
                rxpdo_.control = NONE;
            }
            break;
    }
    
    return true;
}

void Servo::SetPos(int32_t target)
{
    if (rxpdo_.mode == 0x8)
    {
        rxpdo_.pos = target;
    }

    if (rxpdo_.mode == 0x9)
    {
        rxpdo_.vel = target;
    }
}

uint16_t Servo::SdoAccess(void)
{
    uint16_t code = 0x0000;
    switch (ecrt_sdo_request_state(sdo_))
    {
        case EC_REQUEST_UNUSED:         // request was not used yet
            ecrt_sdo_request_read(sdo_); // trigger first read
            break;
        case EC_REQUEST_BUSY:
            break;
        case EC_REQUEST_SUCCESS:
            code = EC_READ_U16(ecrt_sdo_request_data(sdo_));
            ecrt_sdo_request_read(sdo_); // trigger next read
            break;
        case EC_REQUEST_ERROR:
            fprintf(stderr, "Failed to read SDO!\n");
            ecrt_sdo_request_read(sdo_); // retry reading
            break;
    }
    return code;
}

void Servo::Retrieve(uint8_t* domainPtr)
{
    assert(domainPtr != NULL);

    if ( m_PdoOffset_.find(ACTUALSTATUS) != m_PdoOffset_.end())
    {
	    txpdo_.status = EC_READ_U16(domainPtr + *(m_PdoOffset_[ACTUALSTATUS]));
    }

    if (m_PdoOffset_.find(ERRORCODE) != m_PdoOffset_.end())
    {
        txpdo_.errcode  = EC_READ_U16(domainPtr + *(m_PdoOffset_[ERRORCODE]));
    }

    if (rxpdo_.mode == int8_t(0x8))
    {
        // pos
        if (m_PdoOffset_.find(ACTUALPOS) != m_PdoOffset_.end())
        {
            txpdo_.pos = EC_READ_S32(domainPtr + *(m_PdoOffset_[ACTUALPOS]));
        }
    }
    else if(rxpdo_.mode == int8_t(0x9))
    {
        // vel
        if (m_PdoOffset_.find(ACTUALVEL) != m_PdoOffset_.end())
        {
            txpdo_.vel = EC_READ_S32(domainPtr + *(m_PdoOffset_[ACTUALVEL]));
        }
    }
    else
    {
        // tor
    }
}

void Servo::Publisher(uint8_t* domainPtr)
{
    assert(domainPtr != NULL);
    if (m_PdoOffset_.find(TARGETMODE) != m_PdoOffset_.end())
    {
        EC_WRITE_S8(domainPtr + *(m_PdoOffset_[TARGETMODE]), rxpdo_.mode);
    }

    if (m_PdoOffset_.find(TARGETCONTROL) != m_PdoOffset_.end())
    {
    	EC_WRITE_U16(domainPtr + *(m_PdoOffset_[TARGETCONTROL]), rxpdo_.control);
    }

    if(rxpdo_.mode == int8_t(0x8))
    {
        // pos
        EC_WRITE_S32(domainPtr + *(m_PdoOffset_[TARGETPOS]), rxpdo_.pos);
        if (m_PdoOffset_.find(TARGETMAXVEL) != m_PdoOffset_.end())
        {
            EC_WRITE_U32(domainPtr + *(m_PdoOffset_[TARGETMAXVEL]), /*rxpdo_.maxvel */ 1000);
        }

        if (m_PdoOffset_.find(TARGETTVEL) != m_PdoOffset_.end())
        {
            EC_WRITE_U32(domainPtr + *(m_PdoOffset_[TARGETTVEL]), /*rxpdo_.maxvel */ 100);
        }
    }
    else if(rxpdo_.mode == int8_t(0x9))
    {
        // vel
        EC_WRITE_S32(domainPtr + *(m_PdoOffset_[TARGETVEL]), rxpdo_.vel);
        if (m_PdoOffset_.find(TARGETMAXVEL) != m_PdoOffset_.end())
        {
            EC_WRITE_U32(domainPtr + *(m_PdoOffset_[TARGETMAXVEL]), /*rxpdo_.maxvel */ 1000);
        }

        if (m_PdoOffset_.find(TARGETTVEL) != m_PdoOffset_.end())
        {
            EC_WRITE_U32(domainPtr + *(m_PdoOffset_[TARGETTVEL]), /*rxpdo_.maxvel */ 100);
        }
    }
    else
    {
        // tor
    }
}
